package com.grt192.mechanism.cannonbot;

import com.grt192.actuator.GRTCANJaguar;
import com.grt192.controller.pid.AsynchronousPIDController;
import com.grt192.core.Mechanism;
import com.grt192.event.component.GyroEvent;
import com.grt192.event.component.GyroListener;
import com.grt192.event.component.JagEncoderEvent;
import com.grt192.event.component.JagEncoderListener;
import com.grt192.event.controller.PIDEvent;
import com.grt192.event.controller.PIDListener;
import com.grt192.sensor.GRTGyro;
import com.grt192.sensor.canjaguar.GRTJagEncoder;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;

/**
 * 
 * @author anand
 * 
 */
public class CBTankDriveTrain extends Mechanism implements PIDOutput,
		JagEncoderListener, GyroListener, PIDListener {

	public static final int WAIT_INTERVAL = 2;
	public static final int ROBOT_WIDTH = 28; // TODO find

	private double turnP = .10;
	private double turnI = .10;
	private double turnD = .10;

	private double driveP = .10;
	private double driveI = .10;
	private double driveD = .10;

	private GRTCANJaguar leftJaguar;
	private GRTCANJaguar rightJaguar;
	private GRTJagEncoder leftEncoder;
	private GRTJagEncoder rightEncoder;
	private GRTGyro canGyro;

	private double leftWheelX;
	private double leftWheelY;

	private double rightWheelX;
	private double rightWheelY;

	private boolean pointTurn;

	private AsynchronousPIDController turnControl;

	private AsynchronousPIDController leftDriveControl;
	private AsynchronousPIDController rightDriveControl;

	public CBTankDriveTrain(int lfpin, int rfpin, int gyropin) {
		this(new GRTCANJaguar(lfpin), new GRTCANJaguar(rfpin), new GRTGyro(
				gyropin, 5, "cangyro"));
	}

	public CBTankDriveTrain(GRTCANJaguar lfjaguar, GRTCANJaguar rfjaguar,
			GRTGyro canGyro) {
		this.leftJaguar = lfjaguar;
		leftJaguar.start();

		this.rightJaguar = rfjaguar;
		rightJaguar.start();

		this.leftEncoder = new GRTJagEncoder(lfjaguar, 5, "ljagencoder");

		this.rightEncoder = new GRTJagEncoder(rfjaguar, 5, "rjagencoder");

		this.canGyro = canGyro;
		leftEncoder.addEncoderListener(this);
		rightEncoder.addEncoderListener(this);
		canGyro.addGyroListener(this);

		canGyro.start();
		leftEncoder.start();
		rightEncoder.start();

		turnControl = new AsynchronousPIDController(new PIDController(turnP,
				turnI, turnD, canGyro, this));
		leftDriveControl = new AsynchronousPIDController(new PIDController(
				driveP, driveI, driveD, leftEncoder, this));
		rightDriveControl = new AsynchronousPIDController(new PIDController(
				driveP, driveI, driveD, rightEncoder, this));
		pointTurn = true;
		leftDriveControl.addPIDListener(this);
		rightDriveControl.addPIDListener(this);
		turnControl.addPIDListener(this);
		leftDriveControl.start();
		rightDriveControl.start();
		turnControl.start();

		leftWheelX = -ROBOT_WIDTH / 2;
		rightWheelX = ROBOT_WIDTH / 2;
		leftWheelY = rightWheelY = 0;

		addActuator("lfJaguar", leftJaguar);
		addActuator("rbJaguar", rightJaguar);
		addSensor("ljagencoder", leftEncoder);
		addSensor("rjagencoder", rightEncoder);
		addSensor("cangyro", canGyro);

	}

	public double getDriveP() {
		return driveP;
	}

	public void setDriveP(double driveP) {
		this.driveP = driveP;
	}

	public double getDriveI() {
		return driveI;
	}

	public void setDriveI(double driveI) {
		this.driveI = driveI;
	}

	public double getDriveD() {
		return driveD;
	}

	public void setDriveD(double driveD) {
		this.driveD = driveD;
	}

	/**
	 * Drive dts, specifying speed
	 * 
	 * @param leftSpeed
	 * @param rightSpeed
	 */
	public void tankDrive(double leftSpeed, double rightSpeed) {
		this.leftJaguar.enqueueCommand(leftSpeed);
		this.rightJaguar.enqueueCommand(rightSpeed);
	}

	public void driveToPosition(double position) {
		driveToPosition(position, position);
	}

	/**
	 * Drive dts to a specific position, provided where the l&r wheels should be
	 * 
	 * @param leftPosition
	 * @param rightPosition
	 */
	public void driveToPosition(double leftPosition, double rightPosition) {
		stopPIDControl();
		leftDriveControl.getPidThread().setSetpoint(leftPosition);
		rightDriveControl.getPidThread().setSetpoint(rightPosition);
		leftDriveControl.enable();
		rightDriveControl.enable();
	}

	public void driveDistance(double distance) {
		driveDistance(distance, distance);
	}

	/**
	 * Drive l&r wheels a certain distance
	 * 
	 * @param leftDistance
	 * @param rightDistance
	 */
	public void driveDistance(double leftDistance, double rightDistance) {
		driveToPosition(leftEncoder.getState("Distance") + leftDistance,
				rightEncoder.getState("Distance") + rightDistance);
	}

	public void turnTo(double angle) {
		turnTo(angle, true);
	}

	/**
	 * Turn the robot to a specific heading
	 * 
	 * @param angle
	 * @param point
	 */
	public void turnTo(double angle, boolean point) {
		stopPIDControl();
		pointTurn = point;
		turnControl.getPidThread().setSetpoint(angle);
		turnControl.enable();
	}

	public void pidWrite(double output) {
		// Scale speed control to output ratio
		if (pointTurn) {
			tankDrive(output, -output);
		} else {
			tankDrive((1 + output) / 2, 1 - (1 + output) / 2);
		}
	}

	/**
	 * Stop the driveTrains
	 */
	public void stop() {
		stopPIDControl();
		tankDrive(0, 0);
	}

	public double getTurnP() {
		return turnP;
	}

	public void setTurnP(double turnP) {
		this.turnP = turnP;
	}

	public double getTurnI() {
		return turnI;
	}

	public void setTurnI(double turnI) {
		this.turnI = turnI;
	}

	public double getTurnD() {
		return turnD;
	}

	public void setTurnD(double turnD) {
		this.turnD = turnD;
	}

	public boolean isPointTurn() {
		return pointTurn;
	}

	public void setPointTurn(boolean pointTurn) {
		this.pointTurn = pointTurn;
	}

	private void block() {
		try {
			Thread.sleep(WAIT_INTERVAL);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}

	public double getLeftPosition() {
		return leftEncoder.getState("Distance");
	}

	public double getRightPosition() {
		return rightEncoder.getState("Distance");
	}

	public double getHeading() {
		return canGyro.getState("Angle");
	}

	public void changedDirection(JagEncoderEvent e) {
		// TODO Auto-generated method stub

	}

	public void countDidChange(JagEncoderEvent e) {
		double deltaR = e.getDistance() - e.getSource().getState("Previous");
		if (e.getSource().getId().equals("ljagencoder")) {
			// left wheel
			leftWheelX = deltaR * Math.cos(Math.toRadians(getHeading()));
			leftWheelY = deltaR * Math.sin(Math.toRadians(getHeading()));
		} else if (e.getSource().getId().equals("rjagencoder")) {
			// right wheel
			rightWheelX = deltaR * Math.cos(Math.toRadians(getHeading()));
			rightWheelY = deltaR * Math.sin(Math.toRadians(getHeading()));
		}
	}

	public void rotationDidStart(JagEncoderEvent e) {
		// TODO Auto-generated method stub

	}

	public void rotationDidStop(JagEncoderEvent e) {
		// TODO Auto-generated method stub

	}

	public void didAngleChange(GyroEvent e) {

	}

	public void didAngleSpike(GyroEvent e) {
		// TODO Auto-generated method stub

	}

	public void didReceiveAngle(GyroEvent e) {
		// TODO Auto-generated method stub

	}

	public double getLeftWheelX() {
		return leftWheelX;
	}

	public double getLeftWheelY() {
		return leftWheelY;
	}

	public double getRightWheelX() {
		return rightWheelX;
	}

	public double getRightWheelY() {
		return rightWheelY;
	}

	public double getX() {
		return (getLeftWheelX() + getRightWheelX()) / 2;
	}

	public double getY() {
		return (getRightWheelY() + getRightWheelY()) / 2;
	}

	public void stopPIDControl() {
		leftDriveControl.reset();
		rightDriveControl.reset();
		turnControl.reset();
	}

	public void suspendPIDControl() {
		leftDriveControl.disable();
		rightDriveControl.disable();
		turnControl.disable();
	}

	public void onSetpointReached(PIDEvent e) {
		e.getSource().reset();
	}
}
